/*
 * cRKF45.cpp
 *
 *  Created on: Oct 12, 2010
 *      Author: murmeli
 */

#include "cRKF45.h"

RKF45::RKF45() {
	//double ivy, ivdy;
	setName("Runge-Kutta-Fehlberg4(5)");
	//tx and fx are cleared and set from cInfo when active potential is set.
	//If, for some reason, the active potential is not set and method is initiated we
	//set tx and fx to 0 and 10 respectivly.
	/*ivy=0.1;
	ivdy=0;
	setStep(0.01);
	set_tx(0);
	set_fx(10);
	set_ty(ivy);
	set_fy(ivy);
	set_tdy(ivdy);
	set_fdy(ivdy);*/
	setMinerror(1e-12);
	setMaxerror(1e-2);
	setMinstep(1e-12);
	setMaxstep(1.0);
}

RKF45::~RKF45() {
	// TODO Auto-generated destructor stub
}

void RKF45::methodStep(double& step, vector<double> &xx, vector<double> &yy, vector<double> &dy, cDY *DY, cPot *V) {
	double k1,k2,k3,k4,k5,k6,l1,l2,l3,l4,l5,l6,E_est;
	bool done=false;
	while(done==false) {
//Calculate single step
		k1 = dy.back();
		l1 = DY->DE(xx.back(), V->potential(xx.back()), yy.back(), dy.back());

		k2 = dy.back() + (2.0/9.0)*step * l1;
		l2 = DY->DE(xx.back() + (2.0/9.0)*step, V->potential(xx.back() + (2.0/9.0)*step), yy.back() + (2.0/9.0)*step * k1, k2);

		k3 = dy.back() + step*((1.0/12.0) * l1 + (1.0/4.0)*l2);
		l3 = DY->DE(xx.back() + step/3.0, V->potential(xx.back() + step/3.0), yy.back() + step* ((1.0/12.0)*k1 + (1.0/4.0)*k2),k3);

		k4 = dy.back() + step*((69.0/128.0)*l1 -(243.0/128.0)*l2 + (135.0/64.0)*l3);
		l4 = DY->DE(xx.back() + (3.0/4.0)*step, V->potential(xx.back() + (3.0/4.0)*step), yy.back() + step*( (69.0/128.0)*k1 -(243.0/128.0)*k2 + (135.0/64.0)*k3), k4);

		k5 = dy.back() + step*((-17.0/12.0)*l1 + (27.0/4.0)*l2 - (27.0/5.0)*l3 + (16.0/15.0)* l4);
		l5 = DY->DE(xx.back()+step, V->potential(xx.back() + step), yy.back() + step*((-17.0/12.0)*k1 + (27.0/4.0)*k2 - (27.0/5.0)*k3 + (16.0/15.0)* k4), k5);

		k6 = dy.back() + step*( (65.0/432.0)*l1 - (5.0/16.0)*l2 + (13.0/16.0)*l3 + (4.0/27.0)*l4 + (5.0/144.0)*l5);
		l6 = DY->DE(xx.back() + step*(5.0/6.0), V->potential(xx.back() + step*(5.0/6.0)), yy.back() + step*( (65.0/432.0)*k1 - (5.0/16.0)*k2 + (13.0/16.0)*k3 + (4.0/27.0)*k4 + (5.0/144.0)*k5), k6);

//Calculate the error estimate for the step
		E_est = step*((3.0/100.0)*k3 - k1/150.0 - (16.0/75.0)*k4 -k5/20.0 + (6.0/25.0)*k6);
		//cout << "Error = " << E_est <<  " step = " << step <<  " x = " << xx.back() << endl;

//If the error estimate is less than specified maximum error and larger than specified minimum error we are done.
		if(fabs(E_est) < getMaxerror() && fabs(E_est) > getMinerror()) {
			done = true;
		}
//If the error estimate is smaller than specified minimum error we double the step size and calculate the step again (done = false).
		else if(fabs(E_est) < getMinerror()) {
			step = 2.0*step;
                        if(step > getMaxstep()) {step = getMaxstep(); /*cout << "MAX STEPSIZE! E_est = " << E_est << " x = " << xx.back() << endl;*/}
                        //cout << "Hep, minerrorr" << endl;
			done = false;
		}
//If the error estimate is larger than the specified maximum error we calculate a new step size and try to calculate the step again.
		else if(fabs(E_est) > getMaxerror()) {
			step = 0.9*step*pow(getMaxerror()/fabs(E_est),(1.0/5.0));
			//step = step*0.5;
			//cout << "New step size = " << step << " E_est = " << E_est << " xx.back() = " << xx.back() << endl;
			//If the new step size is smaller than the specified minimum step size we throw an exception and calculation stops.
			if(fabs(step) < getMinstep()) {throw(cNumericalError("Minimum step size reached."));}
			done = false;
		}
	}

	yy.push_back(yy.back() + step * ((47.0/450.0)*k1 + (12.0/25.0)*k3 + (32.0/225.0)*k4 + k5/30.0 + (6.0/25.0)*k6));
	dy.push_back(dy.back() + step * ((47.0/450.0)*l1 + (12.0/25.0)*l3 + (32.0/225.0)*l4 + l5/30.0 + (6.0/25.0)*l6));
	xx.push_back(xx.back() + step);
        //cout << "x = " << xx.back() << endl;
}
